/*
 * control_step.cpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */
#include "control_step.hpp"



CONTROL_STEP control_step;

void CONTROL_STEP::Control_Step()
{
	xQueuePeek(queueRCCommand, &rcCommand, 0);					//从队列中获取遥控器数据
		switch(rcCommand.Key[2])
		{
			case 0:
				control_mode = MODE_ALTITUDE;	//手控
				break;
			case 1:
				control_mode = MODE_ALTITUDE;	//半自控
				break;
			case 2:
				control_mode = MODE_POSITION_HOLD;
				break;
			default:break;
		}


	//----------------控制逻辑----------------//

	switch(control_mode)
	{
		case MODE_MANUAL://手控
			break;
		case MODE_ALTITUDE://半自控
			Heli_Attitude_control();
			break;
		case MODE_POSITION_HOLD:
			Heli_Poshold_control();
		case MODE_SPEED://速度操纵
			break;
		case MODE_TARGET://轨迹
			break;
		case MODE_LOST://失控保护
			break;
		default:break;
	}

	//---------------------------------------//
}




void CONTROL_STEP::Control_Step2()
{

}


void CONTROL_STEP::PID_Para_Update(void)
{
	xQueuePeek(queuePID, &pid_msg, 0);
	for(u8 i=0;i<18;i++)
		pid[i].Pid_Set_Para(pid_msg.kp[i], pid_msg.ki[i], pid_msg.kd[i]);
}


void CONTROL_STEP::Tranfer_Data_Updata(void)
{
	xQueuePeek(queueControlTransfer, &control_data, 0);
	for(int i = 0; i < 3; ++i){
		control_data.posCommand[i] = pos_command_ned[i]; //pos_command_ned
		control_data.posFeedback[i] = pos_feedback_ned[i]; //pos_feedback_ned
		control_data.velCommand[i] = vel_command_ned[i]; //vel_command_ned
		control_data.velFeedback[i] = vel_feedback_ned[i]; //vel_feedback_ned[i]
		control_data.accCommand[i] = acc_command_ned[i];
	}
	control_data.angFromRCorOutloop[0] = roll_command_from_rc; //roll_command_from_rc;
	control_data.angFromRCorOutloop[1] = pitch_command_from_rc;
	control_data.angFromRCorOutloop[2] = yaw_rate_command_from_rc;
	control_data.angCommand[0] = roll_command;
	control_data.angCommand[1] = pitch_command;
	control_data.angCommand[2] = yaw_command;
	control_data.angRateCommand[0] = roll_rate_command;
	control_data.angRateCommand[1] = pitch_rate_command;
	control_data.angRateCommand[2] = yaw_rate_command;
	control_data.angRateCommand[3] = target_rate;
	control_data.other[0] = acc_command_body[2];
	for(int i = 0; i < 8; ++i){
		control_data.auto_servo_width[i] = auto_servo_width[i];
	}
	xQueueOverwrite(queueControlTransfer,&control_data);
}

extern "C" void control_main(void *argument)
{
	static u32 cnt = 0;
	control_step.Control_Init();
	control_step.Integral_Init();
	control_step.Set_Limits();
	osDelay(10);
	for(;;)
	{
		osSemaphoreAcquire(semControl,0xffffffff);
		control_step.Control_Step();
		control_step.Control_Step2();
		if(cnt%200) //2s执行一次PID参数更新
			control_step.PID_Para_Update();
		//需要100hz进行
		control_step.Tranfer_Data_Updata();//上传control_data到队列中去
		cnt++;
	}
}
